package nav;

import lejos.nxt.Button;
import lejos.nxt.Motor;
/**Modified version of the 2 wheeled robot class provided by Patrick Diez in for the labs
 * @author Antoine Lennartz
 * @author Patrick Diez*/
public class TwoWheeledRobot {
	public final double  RIGHT_RADIUS = 2.6317, LEFT_RADIUS =2.6323, WIDTH = 13.23;
	protected Motor leftMotor;
	protected Motor rightMotor;
	private double forwardSpeed, rotationSpeed;
	
	public TwoWheeledRobot(Motor leftMotor, Motor rightMotor){
		this.leftMotor = leftMotor;
		this.rightMotor = rightMotor;
	}
	
	
	// accessors
	public int getLeftTacho(){
		return leftMotor.getTachoCount();
	}
	
	public int getRightTacho(){
		return rightMotor.getTachoCount()	;
	}
	
	public void resetTachos(){
		rightMotor.resetTachoCount();
		leftMotor.resetTachoCount();
	}
	
	// mutators
	public void setForwardSpeed(double speed) {
		forwardSpeed = speed;
		setSpeeds(forwardSpeed, rotationSpeed);
	}
	
	
	public void setRotationSpeed(double speed) {
		rotationSpeed = speed;
		setSpeeds(forwardSpeed, rotationSpeed);
	}
	/**sets wheel speeds based on forward a rotation speeds specified in cm/s and degrees/s*
	 * @param forwardSpeed - speed to move forward in cm/s
	 * @param rotationSpeed -  speed to turn by degrees/s positive = clockwise*/
	public void setSpeeds(double forwardSpeed, double rotationSpeed) {
		double leftSpeed, rightSpeed;

		this.forwardSpeed = forwardSpeed;
		this.rotationSpeed = rotationSpeed; 
		
		leftSpeed=forwardSpeed*180.0/(LEFT_RADIUS*Math.PI)+ rotationSpeed*WIDTH/(2*LEFT_RADIUS);
		rightSpeed=forwardSpeed*180.0/(RIGHT_RADIUS*Math.PI)- rotationSpeed*WIDTH/(2*RIGHT_RADIUS);

		// set motor directions
		if (leftSpeed > 0.0 && leftMotor.getMode()!=1)
			leftMotor.forward();
		else if(leftMotor.getMode()!=2 && leftSpeed<0.0){
			leftMotor.backward();
			leftSpeed = -leftSpeed;
		}
		
		if (rightSpeed > 0.0 && rightMotor.getMode()!=1)
			rightMotor.forward();
		else if (rightSpeed < 0.0 && rightMotor.getMode()!=2){
			rightMotor.backward();
			rightSpeed = -rightSpeed;
		}
		// set motor speeds
		if (leftSpeed > 900.0 && leftMotor.getSpeed()!=900)
			leftMotor.setSpeed(900);
		else if(leftMotor.getSpeed()!=(int)leftSpeed && leftSpeed<=900){
			leftMotor.setSpeed((int)leftSpeed);
		}
		
		if (rightSpeed > 900.0 && rightMotor.getSpeed()!=900)
			rightMotor.setSpeed(900);
		else if(rightMotor.getSpeed()!=(int)rightSpeed && rightSpeed<=900)
			rightMotor.setSpeed((int)rightSpeed);
	}

	/**calls Motor.flt() on both wheels
	 * @see lejos.nxt.Motor#flt()
	 * */
	public void floatWheels(){
		rightMotor.forward();
		leftMotor.forward();
		rightMotor.flt();
		leftMotor.flt();
	}
	/**Sets speeds to 0 and calls Motor.stop() on both wheels
	 * @see lejos.nxt.Motor#stop()
	 * */
	public void stop() {
		setSpeeds(0,0);
		rightMotor.stop();
		leftMotor.stop();
	}
	
	
	
	/**used for calibration of mechanical constants
	 * @param distance - dist to travel
	 * @param speed - speed in tacho/s
	 * @param immediateReturn - non blocking if true*/
	public void roll(double distance, int speed, boolean immediateReturn){
		double avgRadius=(LEFT_RADIUS+RIGHT_RADIUS)/2.0;
		leftMotor.setSpeed((int) (avgRadius*speed/LEFT_RADIUS));
		rightMotor.setSpeed((int) (avgRadius*speed/RIGHT_RADIUS));
		leftMotor.rotate((int) (distance/(2*Math.PI*LEFT_RADIUS)*360), true);
		rightMotor.rotate((int) (distance/(2*Math.PI*RIGHT_RADIUS)*360), immediateReturn);
	}
	/**used for calibration of mechanical constants
	 * @param angle - angle to rotate (robot heading)
	 * @param speed - speed in tacho/s
	 * @param immediateReturn - non blocking if true*/
	public void turn(int angle, int speed, boolean immediateReturn){
		double avgRadius=(LEFT_RADIUS+RIGHT_RADIUS)/2.0;
		leftMotor.setSpeed((int) (avgRadius*speed/LEFT_RADIUS));
		rightMotor.setSpeed((int) (avgRadius*speed/RIGHT_RADIUS));
		int lAng=(int)(angle*WIDTH/(2*LEFT_RADIUS));
		int rAng=(int)(angle*WIDTH/(2*RIGHT_RADIUS));
		if(angle>0) rAng=-rAng;
		else lAng=-lAng;
		leftMotor.rotate(lAng, true);
		rightMotor.rotate(rAng, immediateReturn);
	}
	/**pauses the current motion not really in use
	 * @param delay - amount of ms to pause for*/
	public void pauseMotion(int delay) {
		double fSpeed=forwardSpeed, rSpeed=rotationSpeed;		
		setSpeeds(0,0);
		Button.waitForPress(delay);
		setSpeeds(fSpeed, rSpeed);
	}

	public boolean isMoving() {
		return rightMotor.isMoving() || leftMotor.isMoving();
	}


	
}
